#include "Quaternion.h"

Quaternion::Quaternion(void){
    w = 0;
    set(0,0,0);
    magnitude = 0;
}
Quaternion::Quaternion(Vector3f vec){
    w = 0;
    set(vec);
    magnitude = this->length();
}
Quaternion::Quaternion(float x, float y, float z){
    w = 0;
    set(x,y,z);
    magnitude = this->length();
}
Quaternion::Quaternion(float _w, Vector3f vec){
    w = _w;
    set(vec);
    magnitude = this->length();
}
Quaternion::Quaternion(float _w, float x, float y, float z){
    w = _w;
    set(x,y,z);
    magnitude = this->length();
}

Quaternion::~Quaternion(void)
{
}

Quaternion Quaternion::rotateAround(Vector3f axis, float radians){
    Quaternion rotated = *this;
    Quaternion rotation(cos(radians/2),(axis*sin(radians/2)));
    Quaternion rotationConj(rotation);
    rotationConj.inverse();

    *this = rotation*rotated*rotationConj;

    //this->normalize();

    return *this;
}

Vector3f Quaternion::toVector3f(){
    return Vector3f(x,y,z);
}

float Quaternion::normalize(){
    this->length();

    if(magnitude != 0.0f){
        w /= magnitude;
        x /= magnitude;
        y /= magnitude;
        z /= magnitude;
    }
    return magnitude;
}
float Quaternion::length(){
    magnitude = sqrt((w*w)+(x*x)+(y*y)+(z*z));
    return magnitude;
}
void Quaternion::inverse(){
    x *= -1;
    y *= -1;
    z *= -1;
}

Quaternion Quaternion::operator*(const Quaternion &quat2){
    Quaternion quat1 = *this, newQ;

    newQ.w = quat1.w * quat2.w - quat1.x * quat2.x - quat1.y * quat2.y - quat1.z * quat2.z;
    newQ.x = quat1.w * quat2.x + quat1.x * quat2.w + quat1.y * quat2.z - quat1.z * quat2.y;
    newQ.y = quat1.w * quat2.y - quat1.x * quat2.z + quat1.y * quat2.w + quat1.z * quat2.x;
    newQ.z = quat1.w * quat2.z + quat1.x * quat2.y - quat1.y * quat2.x + quat1.z * quat2.w;
    newQ.length();


    return newQ;
}
Quaternion Quaternion::operator=(const Quaternion &quat){
    w = quat.w;
    x = quat.x;
    y = quat.y;
    z = quat.z;
    magnitude = quat.magnitude;
    return *this;
}